import logging
import os

import cv2

import settings as st  # must be imported at first among custom modules and packages

import communication
import params_vision_default as dp_vision  # stands for default_params_vison
from control import control_proxy as ctl_proxy
from control.parameter import Parameters
from control.task import Task, TaskManager
from uart import request
# from vision import back_project as bkpj
from vision.camera import IMG_WIDTH, IMG_HEIGHT
from task_go_straight import TaskGoStraight
from task_play_sound import TaskSound
from task_qr import TaskScanQR
from task_tf_light import TaskScanTF


# ===== ===== ===== ===== ===== =====
# setup logging
# ===== ===== ===== ===== ===== =====
logging.basicConfig(level=logging.DEBUG)
logger = logging.getLogger(__name__)


# ===== ===== ===== ===== ===== =====
# setup devices
# ===== ===== ===== ===== ===== =====
# setup camera
# cap = cv2.VideoCapture(r'C:\_AAA\BlueNet\GK2020\img_and_videos\lane-to-morning.mp4')
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, IMG_WIDTH)  # 设置读取到图像的宽高
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, IMG_HEIGHT)


# setup uart
if os.access('/dev/ttyAMA0', os.F_OK):  # os.F_OK, 文件存在
    comm = communication.Communication(port=r'/dev/ttyAMA0', baudrate=115200)
else:
    comm = communication.Communication(port=r'com4', baudrate=115200)

# 115200 bit/s = 1152 bit/0.01s ~ 115 bytes/0.01s
comm.timeout = 0.005  # s, read timeout, too small value could cause data corruption


# ===== ===== ===== ===== ===== =====
# load runtime parameters
# ===== ===== ===== ===== ===== =====
params_bkpj = Parameters(dp_vision.dp_bkpj, st.PTH_PARAMS_VISION)
# register runtime parameters so the client could change it
ctl_proxy.register_params(st.NAME_PARAMS_BKPJ, params_bkpj)

# logger.debug(repr(ctl_proxy.runtime_params))


# ===== ===== ===== ===== ===== =====
# setup tasks
# ===== ===== ===== ===== ===== =====
task_list = {
    st.LB_GO : TaskGoStraight(cap),
    st.LB_SND: TaskSound(),
    st.LB_SCAN_QR: TaskScanQR(cap),
    st.LB_SCAN_TF: TaskScanTF(cap)
}

tm = TaskManager(task_list)
tm.switch(st.LB_GO)   # test it



# ===== ===== ===== ===== ===== =====
# functions
# ===== ===== ===== ===== ===== =====
def uart_recv():
    df = comm.read_data()
    request.set_dataframe(df)
        
    return df
    

def loop():
    # read serial
    df = uart_recv()
    
    # switch and execute task
    if df is not None:
        tm.switch(df.label)
    label, values = tm.execute()
    # print(label, values)
    
    # send result
    if label:
        comm.write_data(label, values)


# go into loop
from benchmark import Benchmark
bm = Benchmark()
try:
    while True:
        bm.begin()
        loop()
        bm.end()
        if bm.n % 100 == 0:
            bm.print_stats()
finally:
    # comm.close()
    # cap.close()
    pass
    




